#include "hdl_mpu6050.h"

int8_t hdl_mpu6050_i2c_write(uint8_t reg,uint8_t data)
{
    int8_t ret = RET_SUCCESS;
    hal_SW_iic_start();
    hal_SW_iic_wr_byte(MPU6050_ADDRESS);
    if(hal_SW_iic_waitack() != RET_SUCCESS)
    {
        hal_SW_iic_stop();
        ret = RET_FAIL;
    }
    else
    {
        hal_SW_iic_wr_byte(reg);
        hal_SW_iic_waitack();
        hal_SW_iic_wr_byte(data);
        hal_SW_iic_waitack();
        hal_SW_iic_stop();
    }
    return ret;
}

uint8_t hdl_mpu6050_i2c_read(uint8_t reg)
{
    uint8_t recive = 0;
    hal_SW_iic_start();
    hal_SW_iic_wr_byte(MPU6050_ADDRESS);
    if(hal_SW_iic_waitack() != RET_SUCCESS)
    {
        hal_SW_iic_stop();
        return RET_FAIL;
    }
    hal_SW_iic_wr_byte(reg);
    hal_SW_iic_waitack();
    hal_SW_iic_stop();
    hal_SW_iic_start();
    hal_SW_iic_wr_byte(MPU6050_ADDRESS+1);
    if(hal_SW_iic_waitack() != RET_SUCCESS)
    {
        hal_SW_iic_stop();
        return RET_FAIL;
    }
    recive = hal_SW_iic_rd_byte(FALSE);
    hal_SW_iic_stop();
    return recive;
}

int8_t hdl_mpu6050_6dof_init(void)
{
    int8_t ret = RET_SUCCESS;
    uint8_t timeout = 0;
    do
    {
        ret |= hdl_mpu6050_i2c_write(PWR_MGMT_1,0x80);
        sys_delay_ms(30);
        ret |= hdl_mpu6050_i2c_write(SMPLRT_DIV,0x02);
        ret |= hdl_mpu6050_i2c_write(PWR_MGMT_1,0x00);
        ret |= hdl_mpu6050_i2c_write(CONFIGL,0x03);
        ret |= hdl_mpu6050_i2c_write(GYRO_CONFIG,0x18);
        ret |= hdl_mpu6050_i2c_write(ACCEL_CONFIG,0x09);
        timeout++;
        if(0 == timeout % 10)
        {
            timeout = 0;
            ret = RET_ERR;
            break;
        }
    }while(ret != RET_SUCCESS);
    if(ret == RET_SUCCESS)
    {
        DBG_MSG("date:0X%x,MPU6050_PRODUCT_ID:0x%x\r\n",hdl_mpu6050_i2c_read(WHO_AM_I),MPU6050_PRODUCT_ID);

    }
}

